#include "DriveTrain.h"
#include "../../Robotmap.h"
//
DriveTrain::DriveTrain() :
	motorFL(MOTOR_FRONT_LEFT), 
	motorFR(MOTOR_FRONT_RIGHT), 
	motorBL(MOTOR_BACK_LEFT),
	motorBR(MOTOR_BACK_RIGHT),

	encoderFL(ENCODER_FRONT_LEFT_A, ENCODER_FRONT_LEFT_B, false, Encoder::k2X),
	encoderFR(ENCODER_FRONT_RIGHT_A, ENCODER_FRONT_RIGHT_B, false, Encoder::k2X),
	encoderBL(ENCODER_BACK_LEFT_A, ENCODER_BACK_LEFT_B, false, Encoder::k2X),
	encoderBR(ENCODER_BACK_RIGHT_A, ENCODER_BACK_RIGHT_B, false, Encoder::k2X),

	gyro(GYRO),

	accel(ACCELEROMETER) {
}
void DriveTrain::DriveSpeed(float speed, float time) {
	motorFL.Set(speed);
	motorFR.Set(speed);
	motorBL.Set(speed);
	motorBR.Set(speed);
	Timer *timer = new Timer();
	timer->Start();
	while (!timer->HasPeriodPassed(time)) {

	}
	timer->Stop();
	motorFL.Set(0);
	motorFR.Set(0);
	motorBL.Set(0);
	motorBR.Set(0);
}
void DriveTrain::DriveDistance(float distance, float time) {

	motorFL.Set(1);
	motorFR.Set(1);
	motorBL.Set(1);
	motorBR.Set(1);
	encoderFL.Reset();
	Timer *timer = new Timer();
	timer->Start();
	while ((encoderFL.GetDistance() < distance) && (!timer->HasPeriodPassed(time))) {

	}
	motorFL.Set(0);
	motorFR.Set(0);
	motorBL.Set(0);
	motorBR.Set(0);

}

void DriveTrain::RotateSpeed(float speed, float time) {
	//If Speed > 0, clockwise
	//If speed < 0, Counter-Clockwise
	motorFL.Set(speed);
	motorFR.Set(-speed);
	motorBL.Set(speed);
	motorBR.Set(-speed);
	Timer *timer = new Timer();
	timer->Start();
	while (!timer->HasPeriodPassed(time)) {

	}
	timer->Stop();
	motorFL.Set(0);
	motorFR.Set(0);
	motorBL.Set(0);
	motorBR.Set(0);
}

void DriveTrain::SetMotor(WheelType wheel, double speed)
{
	switch (wheel)
	{
		FrontLeft:
			motorFL.Set(speed);
			break;
		FrontRight:
			motorFR.Set(speed);
			break;
		BackLeft:
			motorBL.Set(speed);
			break;
		BackRight:
			motorBR.Set(speed);
			break;
	}
}

